/*! \file rungekutta.cpp

\brief Ordinary differential equations solver with 4 order runge kutta

Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.
*/

#include "../dynlib.h"

/*
\brief Ordinary differential equations solver with 4 order runge kutta

Date of creation: 2007-02-03

INPUT:
\param MAT SimParam: Simulation Parameters 4 x 6
\param MAT X: State matrix 4 x pt
\param int i: Actual point
\param double u: Control action

OUTPUT:

\param MAT X: i+1 position modified.
*/
void odesolver(Matrix &SimParam,Matrix &X,int i,Matrix &u)
{
	int j,fin;
	fin=(int)(SimParam(4,4)+SimParam(4,3));
	
	ColumnVector k1(fin), k2(fin), k3(fin), k4(fin);
	Matrix mtemp(fin,1);
		
	//k1=dynsimdic[function](SimParam,X[:,i],u)*step
	odefunc(SimParam,X,i,u,i,k1);
	k1=k1*SimParam(3,3);
	
	
	//k2=dynsimdic[function](SimParam,X[:,i]+0.5*k1,u)*step
	mtemp.Column(1)=X.column(i)+0.5*k1;
	odefunc(SimParam,mtemp,1,u,i,k2);
	k2=k2*SimParam(3,3);

	
	//k3=dynsimdic[function](SimParam,X[:,i]+0.5*k2,u)*step
	mtemp.Column(1)=X.column(i)+0.5*k2;
	odefunc(SimParam,mtemp,1,u,i,k3);
	k3=k3*SimParam(3,3);
	
	
	//k4=dynsimdic[function](SimParam,X[:,i]+k3,u)*step
	mtemp.Column(1)=X.column(i)+k3;
	odefunc(SimParam,mtemp,1,u,i,k4);
	k4=k4*SimParam(3,3);
	
	//X[:,i+1]=X[:,i]+1.0/6.0*(k1+2*k2+2*k3+k4)
	X.column(i+1)=X.column(i)+1.0/6.0*(k1+2*k2+2*k3+k4);
	

}
